#include "mpu6050.h"
#include "Driver/CLOCK/clock.h"
/****************************************************************************************************************/
/*                                        Private Definitions                                                  */
/****************************************************************************************************************/

#define MPU6050_I2C_TIMEOUT     1000    // I2C timeout counter
#define MPU6050_DELAY_MS        100     // General delay

/****************************************************************************************************************/
/*                                        Private Functions                                                    */
/****************************************************************************************************************/

/**
 * @brief Delay function
 * @param ms: delay time in milliseconds
 */
static void MPU6050_Delay_MS(uint32_t ms)
{
    mspm0_delay_ms(ms);
}

/**
 * @brief Wait for I2C transaction to complete
 * @return 0 if success, 1 if timeout
 */
static uint8_t MPU6050_I2C_Wait_Complete(void)
{
    uint32_t timeout = MPU6050_I2C_TIMEOUT;
    
    while (DL_I2C_getControllerStatus(I2C_MPU6050_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS) {
        timeout--;
        if (timeout == 0) {
            // Clear any error states before returning
            DL_I2C_flushControllerTXFIFO(I2C_MPU6050_INST);
            DL_I2C_flushControllerRXFIFO(I2C_MPU6050_INST);
            return 1;  // Timeout error
        }
        // Small delay to prevent busy waiting
        for(volatile int i = 0; i < 10; i++);
    }
    return 0;  // Success
}

/****************************************************************************************************************/
/*                                        Low-level I2C Functions                                              */
/****************************************************************************************************************/

/**
 * @brief Write a single byte to MPU6050 register
 * @param reg: register address
 * @param data: data to write
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Write_Byte(uint8_t reg, uint8_t data)
{
    uint8_t tx_data[2] = {reg, data};
    
    // Reset any previous transaction
    DL_I2C_flushControllerTXFIFO(I2C_MPU6050_INST);
    
    // Start I2C transaction
    DL_I2C_startControllerTransfer(I2C_MPU6050_INST, MPU6050_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 2);
    
    // Send register address and data
    DL_I2C_fillControllerTXFIFO(I2C_MPU6050_INST, &tx_data[0], 2);
    
    // Wait for transaction to complete
    if (MPU6050_I2C_Wait_Complete() != 0) {
        return 1;  // Timeout error
    }
    
    return 0;  // Success
}

/**
 * @brief Read a single byte from MPU6050 register
 * @param reg: register address
 * @return read data, 0xFF if error
 */
uint8_t MPU6050_Read_Byte(uint8_t reg)
{
    uint8_t rx_data = 0;
    uint32_t timeout = MPU6050_I2C_TIMEOUT;
    
    // Reset any previous transaction
    DL_I2C_flushControllerTXFIFO(I2C_MPU6050_INST);
    DL_I2C_flushControllerRXFIFO(I2C_MPU6050_INST);
    
    // First, write register address
    DL_I2C_startControllerTransfer(I2C_MPU6050_INST, MPU6050_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    DL_I2C_fillControllerTXFIFO(I2C_MPU6050_INST, &reg, 1);
    
    // Wait for write to complete
    if (MPU6050_I2C_Wait_Complete() != 0) {
        return 0xFF;  // Timeout error
    }
    
    // Then, read data
    DL_I2C_startControllerTransfer(I2C_MPU6050_INST, MPU6050_ADDR, DL_I2C_CONTROLLER_DIRECTION_RX, 1);
    
    // Wait for data to be available
    while (DL_I2C_isControllerRXFIFOEmpty(I2C_MPU6050_INST)) {
        timeout--;
        if (timeout == 0) {
            return 0xFF; // Timeout error
        }
    }
    
    rx_data = DL_I2C_receiveControllerData(I2C_MPU6050_INST);
    
    // Wait for transaction to complete
    MPU6050_I2C_Wait_Complete();
    
    return rx_data;
}

/**
 * @brief Write multiple bytes to MPU6050 registers
 * @param reg: starting register address
 * @param len: number of bytes to write
 * @param buf: data buffer
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Write_Len(uint8_t reg, uint8_t len, uint8_t *buf)
{
    uint8_t i;
    
    // Reset any previous transaction
    DL_I2C_flushControllerTXFIFO(I2C_MPU6050_INST);
    
    // Start I2C transaction
    DL_I2C_startControllerTransfer(I2C_MPU6050_INST, MPU6050_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, len + 1);
    
    // Send register address
    DL_I2C_fillControllerTXFIFO(I2C_MPU6050_INST, &reg, 1);
    
    // Send data
    for (i = 0; i < len; i++) {
        DL_I2C_fillControllerTXFIFO(I2C_MPU6050_INST, &buf[i], 1);
    }
    
    // Wait for transaction to complete
    if (MPU6050_I2C_Wait_Complete() != 0) {
        return 1;  // Timeout error
    }
    
    return 0;  // Success
}

/**
 * @brief Read multiple bytes from MPU6050 registers
 * @param reg: starting register address
 * @param len: number of bytes to read
 * @param buf: data buffer
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Read_Len(uint8_t reg, uint8_t len, uint8_t *buf)
{
    uint8_t i;
    
    // Reset any previous transaction
    DL_I2C_flushControllerTXFIFO(I2C_MPU6050_INST);
    DL_I2C_flushControllerRXFIFO(I2C_MPU6050_INST);
    
    // First, write register address
    DL_I2C_startControllerTransfer(I2C_MPU6050_INST, MPU6050_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    DL_I2C_fillControllerTXFIFO(I2C_MPU6050_INST, &reg, 1);
    
    // Wait for write to complete
    if (MPU6050_I2C_Wait_Complete() != 0) {
        return 1;  // Timeout error
    }
    
    // Then, read data
    DL_I2C_startControllerTransfer(I2C_MPU6050_INST, MPU6050_ADDR, DL_I2C_CONTROLLER_DIRECTION_RX, len);
    
    // Read all data
    for (i = 0; i < len; i++) {
        // Wait for data to be available
        uint32_t timeout = MPU6050_I2C_TIMEOUT;
        while (DL_I2C_isControllerRXFIFOEmpty(I2C_MPU6050_INST)) {
            timeout--;
            if (timeout == 0) {
                return 1; // Timeout error
            }
        }
        buf[i] = DL_I2C_receiveControllerData(I2C_MPU6050_INST);
    }
    
    // Wait for transaction to complete
    MPU6050_I2C_Wait_Complete();
    
    return 0;  // Success
}

/****************************************************************************************************************/
/*                                        Initialization and Configuration                                     */
/****************************************************************************************************************/

/**
 * @brief Test MPU6050 connection
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Test_Connection(void)
{
    uint8_t device_id = MPU6050_Read_Byte(MPU_DEVICE_ID_REG);
    return (device_id == MPU6050_DEVICE_ID || 
            device_id == MPU6500_DEVICE_ID || 
            device_id == MPU9250_DEVICE_ID) ? 0 : 1;
}

/**
 * @brief Reset MPU6050
 */
void MPU6050_Reset(void)
{
    MPU6050_Write_Byte(MPU_PWR_MGMT1_REG, 0x80);  // Reset device
    MPU6050_Delay_MS(MPU6050_DELAY_MS);
}

/**
 * @brief Enable/Disable MPU6050 sleep mode
 * @param enable: true to enable sleep, false to disable
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Sleep(bool enable)
{
    uint8_t reg_val = MPU6050_Read_Byte(MPU_PWR_MGMT1_REG);
    if (enable) {
        reg_val |= 0x40;  // Set sleep bit
    } else {
        reg_val &= ~0x40; // Clear sleep bit
    }
    return MPU6050_Write_Byte(MPU_PWR_MGMT1_REG, reg_val);
}

/**
 * @brief Initialize MPU6050
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Init(void)
{
    // Test connection first
    if (MPU6050_Test_Connection() != 0) {
        return 1;  // Connection failed
    }
    
    // Reset device
    MPU6050_Reset();
    
    // Wake up device (clear sleep bit)
    if (MPU6050_Write_Byte(MPU_PWR_MGMT1_REG, 0x00) != 0) {
        return 1;
    }
    
    // Set gyroscope full scale range to ±2000 dps
    if (MPU6050_Set_Gyro_FSR(MPU_GYRO_FSR_2000DPS) != 0) {
        return 1;
    }
    
    // Set accelerometer full scale range to ±2g
    if (MPU6050_Set_Accel_FSR(MPU_ACCEL_FSR_2G) != 0) {
        return 1;
    }
    
    // Set sample rate to 100Hz
    if (MPU6050_Set_Rate(100) != 0) {
        return 1;
    }
    
    // Disable all interrupts
    if (MPU6050_Write_Byte(MPU_INT_EN_REG, 0x00) != 0) {
        return 1;
    }
    
    // Disable I2C master mode
    if (MPU6050_Write_Byte(MPU_USER_CTRL_REG, 0x00) != 0) {
        return 1;
    }
    
    // Disable FIFO
    if (MPU6050_Write_Byte(MPU_FIFO_EN_REG, 0x00) != 0) {
        return 1;
    }
    
    // Configure interrupt pin (active low)
    if (MPU6050_Write_Byte(MPU_INTBP_CFG_REG, 0x80) != 0) {
        return 1;
    }
    
    // Set clock source to PLL with X axis gyroscope reference
    if (MPU6050_Write_Byte(MPU_PWR_MGMT1_REG, 0x01) != 0) {
        return 1;
    }
    
    // Enable all sensors (accelerometer and gyroscope)
    if (MPU6050_Write_Byte(MPU_PWR_MGMT2_REG, 0x00) != 0) {
        return 1;
    }
    
    return 0;  // Success
}

/**
 * @brief Set gyroscope full scale range
 * @param fsr: full scale range (0-3)
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Set_Gyro_FSR(uint8_t fsr)
{
    return MPU6050_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3);
}

/**
 * @brief Set accelerometer full scale range
 * @param fsr: full scale range (0-3)
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Set_Accel_FSR(uint8_t fsr)
{
    return MPU6050_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3);
}

/**
 * @brief Set digital low pass filter
 * @param lpf: filter frequency in Hz
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Set_LPF(uint16_t lpf)
{
    uint8_t data = 0;
    
    if (lpf >= 188) data = 1;
    else if (lpf >= 98) data = 2;
    else if (lpf >= 42) data = 3;
    else if (lpf >= 20) data = 4;
    else if (lpf >= 10) data = 5;
    else data = 6;
    
    return MPU6050_Write_Byte(MPU_CFG_REG, data);
}

/**
 * @brief Set sample rate
 * @param rate: sample rate in Hz (4-1000)
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Set_Rate(uint16_t rate)
{
    uint8_t data;
    
    if (rate > 1000) rate = 1000;
    if (rate < 4) rate = 4;
    
    data = 1000 / rate - 1;
    
    if (MPU6050_Write_Byte(MPU_SAMPLE_RATE_REG, data) != 0) {
        return 1;
    }
    
    // Auto-set LPF to half of sample rate
    return MPU6050_Set_LPF(rate / 2);
}

/**
 * @brief Configure FIFO
 * @param sens: sensor selection
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Set_FIFO(uint8_t sens)
{
    return MPU6050_Write_Byte(MPU_FIFO_EN_REG, sens);
}

/****************************************************************************************************************/
/*                                        Data Reading Functions                                               */
/****************************************************************************************************************/

/**
 * @brief Get temperature data
 * @return temperature value (multiplied by 100)
 */
int16_t MPU6050_Get_Temperature(void)
{
    uint8_t buf[2];
    int16_t raw;
    float temp;
    
    if (MPU6050_Read_Len(MPU_TEMP_OUTH_REG, 2, buf) != 0) {
        return 0;  // Error
    }
    
    raw = ((int16_t)buf[0] << 8) | buf[1];
    temp = 36.53 + ((float)raw) / 340.0;
    
    return (int16_t)(temp * 100);  // Return temperature * 100
}

/**
 * @brief Get gyroscope data
 * @param gx: X-axis gyroscope data
 * @param gy: Y-axis gyroscope data
 * @param gz: Z-axis gyroscope data
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Get_Gyroscope(int16_t *gx, int16_t *gy, int16_t *gz)
{
    uint8_t buf[6];
    
    if (MPU6050_Read_Len(MPU_GYRO_XOUTH_REG, 6, buf) != 0) {
        return 1;  // Error
    }
    
    *gx = ((int16_t)buf[0] << 8) | buf[1];
    *gy = ((int16_t)buf[2] << 8) | buf[3];
    *gz = ((int16_t)buf[4] << 8) | buf[5];
    
    return 0;  // Success
}

/**
 * @brief Get accelerometer data
 * @param ax: X-axis accelerometer data
 * @param ay: Y-axis accelerometer data
 * @param az: Z-axis accelerometer data
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Get_Accelerometer(int16_t *ax, int16_t *ay, int16_t *az)
{
    uint8_t buf[6];
    
    if (MPU6050_Read_Len(MPU_ACCEL_XOUTH_REG, 6, buf) != 0) {
        return 1;  // Error
    }
    
    *ax = ((int16_t)buf[0] << 8) | buf[1];
    *ay = ((int16_t)buf[2] << 8) | buf[3];
    *az = ((int16_t)buf[4] << 8) | buf[5];
    
    return 0;  // Success
}

/**
 * @brief Read all sensor data at once
 * @param data: pointer to MPU6050_Data_t structure
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Read_All_Data(MPU6050_Data_t *data)
{
    uint8_t buf[14];  // 6 bytes accel + 2 bytes temp + 6 bytes gyro
    
    if (MPU6050_Read_Len(MPU_ACCEL_XOUTH_REG, 14, buf) != 0) {
        return 1;  // Error
    }
    
    // Parse accelerometer data
    data->accel_x = ((int16_t)buf[0] << 8) | buf[1];
    data->accel_y = ((int16_t)buf[2] << 8) | buf[3];
    data->accel_z = ((int16_t)buf[4] << 8) | buf[5];
    
    // Parse temperature data
    data->temp = ((int16_t)buf[6] << 8) | buf[7];
    
    // Parse gyroscope data
    data->gyro_x = ((int16_t)buf[8] << 8) | buf[9];
    data->gyro_y = ((int16_t)buf[10] << 8) | buf[11];
    data->gyro_z = ((int16_t)buf[12] << 8) | buf[13];
    
    return 0;  // Success
}

/****************************************************************************************************************/
/*                                        Debug Functions                                                      */
/****************************************************************************************************************/

/**
 * @brief Simplified MPU6050 initialization for debugging
 * @return 0 if success, 1 if error
 */
uint8_t MPU6050_Debug_Init(void)
{
    // Add longer delays for debugging
    MPU6050_Delay_MS(200);
    
    // Test basic communication first
    uint8_t device_id = MPU6050_Read_Byte(MPU_DEVICE_ID_REG);
    if (device_id != MPU6050_DEVICE_ID && 
        device_id != MPU6500_DEVICE_ID && 
        device_id != MPU9250_DEVICE_ID) {
        return 1;  // Connection failed
    }
    
    // Reset device
    if (MPU6050_Write_Byte(MPU_PWR_MGMT1_REG, 0x80) != 0) {
        return 2;  // Reset failed
    }
    MPU6050_Delay_MS(200);  // Longer delay after reset
    
    // Wake up device
    if (MPU6050_Write_Byte(MPU_PWR_MGMT1_REG, 0x00) != 0) {
        return 3;  // Wake up failed
    }
    MPU6050_Delay_MS(100);
    
    // Basic configuration only
    if (MPU6050_Write_Byte(MPU_PWR_MGMT2_REG, 0x00) != 0) {
        return 4;  // Sensor enable failed
    }
    
    return 0;  // Success
}

/**
 * @brief Scan I2C bus for devices (debugging purpose)
 */
void MPU6050_Debug_I2C_Scan(void)
{
    // This function would scan I2C addresses from 0x08 to 0x77
    // For now, we'll just test the MPU6050 address
    for (uint8_t addr = 0x68; addr <= 0x69; addr++) {
        // Try to read from this address
        uint8_t test_data = 0xFF;
        
        // Start I2C transaction
        DL_I2C_flushControllerTXFIFO(I2C_MPU6050_INST);
        DL_I2C_flushControllerRXFIFO(I2C_MPU6050_INST);
        
        DL_I2C_startControllerTransfer(I2C_MPU6050_INST, addr, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
        uint8_t reg_addr = MPU_DEVICE_ID_REG;
        DL_I2C_fillControllerTXFIFO(I2C_MPU6050_INST, &reg_addr, 1);
        
        if (MPU6050_I2C_Wait_Complete() == 0) {
            // Address responded, this is our device
            break;
        }
    }
}
